/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.kreig133.spine.algorithm;

import com.kreig133.spine.abstractmodel.*;
import com.kreig133.spine.simplemodel.SimpleEnvironment;
import com.kreig133.spine.simplemodel.SimpleState;
/**
 *
 * @author C.C.-fag
 */
public class AlgorithmTangentWithMG  extends AbstractAlgorithm{

    public AlgorithmTangentWithMG() {
    }
   
    public AbstractState calculate(AbstractEnvironment env, AbstractState state) {

        SimpleState next = (SimpleState) ((SimpleEnvironment) env).getNext();
        SimpleState previous = (SimpleState) ((SimpleEnvironment) env).getPrevious();

        double downForce = 0.0;
        if (next != null) {
            downForce = - calculateFiTg(600.0, Math.PI, -0.5,
                    next.getCoordinate()- (  (SimpleState) state).getCoordinate() );
        }
        SimpleState simpleState = (SimpleState) state;
        double deltaT = ((SimpleEnvironment) env).getSimulationStep();
        double speed  = simpleState.getSpeed() + (( previous.getUpForce()-downForce )/
                velebra.getWeight() - 9.81)*deltaT;
//        System.out.println(simpleState.getCoordinate());
//        System.out.println(simpleState.getSpeed());
//        System.out.println(deltaT);
//        System.out.println(downForce);
        double x = simpleState.getCoordinate() +  speed* deltaT +
                deltaT * deltaT * (( previous.getUpForce()-downForce )/
                velebra.getWeight()-9.81)/2;

        if (x < previous.getCoordinate()) {
//            System.out.println("Настрой логи уже, тут косяки есть!");
            x = previous.getCoordinate()+0.00001;
        }
//        System.out.println("------------------------------------");
//        System.out.println(Thread.currentThread().getName()+" x =   "+x);
//        System.out.println("Previous str = "+previous.getUpForce());
//        System.out.println("downForce = "+downForce);
        return new SimpleState(x, downForce, speed);
    }

    public double calculateFiTg(double a, double b, double c, double y) {
        return a * Math.tan(b * (y + c));
    }
}
